#include "stdafx.h"
#include "gp_Vec.h"
#include "gp_Trsf.h"
#include "gp_Dir2d.h"
#include "gp_XY.h"
#include "gp_Pnt2d.h"
#include "gp_Ax2d.h"
#include "gp_Trsf2d.h"
#include "gp_Vec2d.h"
#include "gp_dir.h"
#include "gp.h"
#include "gp_Ax1.h"
#include "gp_Ax2.h"

 gp_Dir::gp_Dir () : coord(1.,0.,0.)
{ 
}

 gp_Dir::gp_Dir (const gp_Vec& V)
{
  const gp_XYZ& XYZ = V.XYZ();
  double X = XYZ.X();
  double Y = XYZ.Y();
  double Z = XYZ.Z();
  double D = sqrt(X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 gp_Dir::gp_Dir (const gp_XYZ& XYZ)
{
  double X = XYZ.X();
  double Y = XYZ.Y();
  double Z = XYZ.Z();
  double D = sqrt(X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 gp_Dir::gp_Dir (const double Xv,
		       const double Yv,
		       const double Zv)
{
  double D = sqrt (Xv * Xv + Yv * Yv + Zv * Zv);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(Xv / D);
  coord.SetY(Yv / D);
  coord.SetZ(Zv / D);
}

 void gp_Dir::SetCoord (const int Index,
			      const double Xi)
{
  double X = coord.X();
  double Y = coord.Y();
  double Z = coord.Z();
  Standard_OutOfRange_Raise_if(Index < 1 || Index > 3, " ");
  if      (Index == 1)  X = Xi;
  else if (Index == 2)  Y = Xi;
  else                  Z = Xi;
  double D = sqrt (X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 void gp_Dir::SetCoord (const double Xv,
			      const double Yv,
			      const double Zv) {
  double D = sqrt(Xv * Xv + Yv * Yv + Zv * Zv);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(Xv / D);
  coord.SetY(Yv / D);
  coord.SetZ(Zv / D);
}

 void gp_Dir::SetX (const double X)
{
  double Y = coord.Y();
  double Z = coord.Z();
  double D = sqrt (X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 void gp_Dir::SetY (const double Y)
{
  double Z = coord.Z();
  double X = coord.X();
  double D = sqrt (X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 void gp_Dir::SetZ (const double Z)
{
  double X = coord.X();
  double Y = coord.Y();
  double D = sqrt (X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 void gp_Dir::SetXYZ (const gp_XYZ& XYZ)
{
  double X = XYZ.X();
  double Y = XYZ.Y();
  double Z = XYZ.Z();
  double D = sqrt(X * X + Y * Y + Z * Z);
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.SetX(X / D);
  coord.SetY(Y / D);
  coord.SetZ(Z / D);
}

 double gp_Dir::Coord (const int Index) const
{ return coord.Coord (Index);}

  void gp_Dir::Coord (double& Xv,
			    double& Yv,
			    double& Zv) const
{ coord.Coord (Xv, Yv, Zv); }

  double gp_Dir::X() const
{ return coord.X() ; }

  double gp_Dir::Y() const
{ return coord.Y() ; }

  double gp_Dir::Z() const
{ return coord.Z() ; }

  const gp_XYZ& gp_Dir::XYZ () const
{ return coord; }

 bool gp_Dir::IsEqual
(const gp_Dir& Other, 
 const double AngularTolerance) const
{ return   Angle (Other) <= AngularTolerance; }    

 bool gp_Dir::IsNormal
(const gp_Dir& Other,
 const double AngularTolerance) const
{
  double Ang = M_PI / 2.0 - Angle (Other);
  if (Ang < 0) Ang = - Ang;
  return   Ang <= AngularTolerance;
}    

 bool gp_Dir::IsOpposite
(const gp_Dir& Other,
 const double AngularTolerance) const
{ return M_PI - Angle (Other) <= AngularTolerance; }    

 bool gp_Dir::IsParallel
(const gp_Dir& Other, 
 const double AngularTolerance) const
{
  double Ang = Angle (Other);
  return Ang <= AngularTolerance || M_PI - Ang <= AngularTolerance;
}    

 void gp_Dir::Cross (const gp_Dir& Right)
{ 
  coord.Cross (Right.coord); 
  double D = coord.Modulus ();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.Divide (D);
}

 gp_Dir gp_Dir::Crossed (const gp_Dir& Right) const
{
  gp_Dir V = *this;
  V.coord.Cross (Right.coord);
  double D = V.coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  V.coord.Divide (D);
  return V;
}

 void gp_Dir::CrossCross (const gp_Dir& V1,
				const gp_Dir& V2)
{ 
  coord.CrossCross (V1.coord, V2.coord);
  double D = coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  coord.Divide(D);
}

 gp_Dir gp_Dir::CrossCrossed (const gp_Dir& V1,
				    const gp_Dir& V2) const
{
  gp_Dir V = *this;
  (V.coord).CrossCross (V1.coord, V2.coord);
  double D = V.coord.Modulus();
  Standard_ConstructionError_Raise_if(D <= gp::Resolution(), "");
  V.coord.Divide(D);
  return V;
}

 double gp_Dir::Dot (const gp_Dir& Other) const
{ return coord.Dot (Other.coord); }

 double gp_Dir::DotCross (const gp_Dir& V1,
				       const gp_Dir& V2)  const
{ return coord.Dot (V1.coord.Crossed (V2.coord)); } 

 void gp_Dir::Reverse ()
{ coord.Reverse(); }

 gp_Dir gp_Dir::Reversed () const { 
  gp_Dir V = *this;
  V.coord.Reverse ();
  return V;
}

 void gp_Dir::Rotate (const gp_Ax1& A1, const double Ang)
{
  gp_Trsf T;
  T.SetRotation (A1, Ang);
  coord.Multiply (T.HVectorialPart ());
}

 gp_Dir gp_Dir::Rotated (const gp_Ax1& A1,
			       const double Ang) const
{
  gp_Dir V = *this;
  V.Rotate (A1, Ang);
  return V;
}

 gp_Dir gp_Dir::Transformed (const gp_Trsf& T) const
{
  gp_Dir V = *this;
  V.Transform (T);
  return V;
} 

 double gp_Dir::Angle (const gp_Dir& Other) const
 {
	 //    Commentaires :
	 //    Au dessus de 45 degres l'arccos donne la meilleur precision pour le
	 //    calcul de l'angle. Sinon il vaut mieux utiliser l'arcsin.
	 //    Les erreurs commises sont loin d'etre negligeables lorsque l'on est
	 //    proche de zero ou de 90 degres.
	 //    En 3d les valeurs angulaires sont toujours positives et comprises entre
	 //    0 et PI
	 double Cosinus = coord.Dot (Other.coord);
	 if (Cosinus > -0.70710678118655 && Cosinus < 0.70710678118655)
		 return acos (Cosinus);
	 else {
		 double Sinus = (coord.Crossed (Other.coord)).Modulus ();
		 if(Cosinus < 0.0)  return M_PI - asin (Sinus);
		 else               return      asin (Sinus);
	 }
 }

 double gp_Dir::AngleWithRef (const gp_Dir& Other,
	 const gp_Dir& Vref) const
 {
	 double Ang;
	 gp_XYZ XYZ = coord.Crossed (Other.coord);
	 double Cosinus = coord.Dot(Other.coord);
	 double Sinus   = XYZ.Modulus ();
	 if (Cosinus > -0.70710678118655 && Cosinus < 0.70710678118655)
		 Ang =  acos (Cosinus);
	 else {
		 if(Cosinus < 0.0)  Ang = M_PI - asin (Sinus);
		 else               Ang =      asin (Sinus);
	 }
	 if (XYZ.Dot (Vref.coord) >= 0.0)  return  Ang;
	 else                              return -Ang;
 } 

 void gp_Dir::Mirror (const gp_Dir& V)
 {
	 const gp_XYZ& XYZ = V.coord;
	 double A = XYZ.X();
	 double B = XYZ.Y();
	 double C = XYZ.Z();
	 double X = coord.X();
	 double Y = coord.Y();
	 double Z = coord.Z();
	 double M1 = 2.0 * A * B;
	 double M2 = 2.0 * A * C;
	 double M3 = 2.0 * B * C;
	 double XX = ((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z;
	 double YY = M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z;
	 double ZZ = M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z;
	 coord.SetCoord(XX,YY,ZZ);
 }

 void gp_Dir::Mirror (const gp_Ax1& A1)
 {
	 const gp_XYZ& XYZ = A1.Direction().coord;
	 double A = XYZ.X();
	 double B = XYZ.Y();
	 double C = XYZ.Y();
	 double X = coord.X();
	 double Y = coord.Y();
	 double Z = coord.Z();
	 double M1 = 2.0 * A * B;
	 double M2 = 2.0 * A * C;
	 double M3 = 2.0 * B * C;
	 double XX = ((2.0 * A * A) - 1.0) * X + M1 * Y + M2 * Z;
	 double YY = M1 * X + ((2.0 * B * B) - 1.0) * Y + M3 * Z;
	 double ZZ = M2 * X + M3 * Y + ((2.0 * C * C) - 1.0) * Z;
	 coord.SetCoord(XX,YY,ZZ);
 }

 void gp_Dir::Mirror (const gp_Ax2& A2)
 {
	 const gp_Dir& Vz = A2.Direction();
	 Mirror(Vz);
	 Reverse();
 }

 void gp_Dir::Transform (const gp_Trsf& T)
 {
	 if (T.Form() == gp_Identity ||  T.Form() == gp_Translation)    { }
	 else if (T.Form() == gp_PntMirror) { coord.Reverse(); }
	 else if (T.Form() == gp_Scale) { 
		 if (T.ScaleFactor() < 0.0) { coord.Reverse(); }
	 }
	 else {
		 coord.Multiply (T.HVectorialPart());
		 double D = coord.Modulus();
		 coord.Divide(D);
		 if (T.ScaleFactor() < 0.0) { coord.Reverse(); }
	 } 
 }

 gp_Dir gp_Dir::Mirrored (const gp_Dir& V) const
 {
	 gp_Dir Vres = *this;
	 Vres.Mirror (V);
	 return Vres;
 }

 gp_Dir gp_Dir::Mirrored (const gp_Ax1& A1) const
 {
	 gp_Dir V = *this;
	 V.Mirror (A1);
	 return V;
 }

 gp_Dir gp_Dir::Mirrored (const gp_Ax2& A2) const
 {
	 gp_Dir V = *this;
	 V.Mirror (A2);
	 return V;
 }
